import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler # 欧拉角转4元素函数
import math

class StaticTfBrodercaster(Node):
    def __init__(self):
        super().__init__('static_tf')
        self.static_tf = StaticTransformBroadcaster(self)
        self.publish()

    def publish(self):
        transform = TransformStamped()
        transform.header.frame_id = "base_link"
        transform.child_frame_id = "camera_link"
        transform.header.stamp = self.get_clock().now().to_msg()

        transform.transform.translation.x = 0.5
        transform.transform.translation.x = 0.3
        transform.transform.translation.x = 0.6
        
        # 欧拉角转4元素 q = x y z w
        q = quaternion_from_euler(math.radians(180),0,0)

        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.w = q[2]
        transform.transform.rotation.z = q[3]

        self.static_tf.sendTransform(transform)

        self.get_logger().info(f'发布静态tf{transform}')

def main():
    rclpy.init()
    node = StaticTfBrodercaster()
    rclpy.spin(node)
    rclpy.shutdown()